#include <core/factor/factor_analytic.h>

Inheritance diagram for wolf::FactorAnalytic:

Public Member Functions
	FactorAnalytic (const std::string &_type, const FactorTopology &_top, const Eigen::VectorXd &_measurement, const Eigen::MatrixXd &_measurement_sqrt_information_upper, const NodeStateBlocksPtrList &_node_state_block_list, const ProcessorBasePtr &_processor_ptr, const std::vector< StateBlockPtr > &_state_ptrs, bool _apply_loss_function, FactorStatus _status)

	~FactorAnalytic () override=default

bool	evaluate (double const const parameters, double residuals, double *jacobians) const override
	Evaluate the factor given the input parameters and returning the residuals and jacobians. More...

bool	evaluate (const std::vector< const double * > &_states_ptr, Eigen::VectorXd &_residual, std::vector< Eigen::MatrixXd > &_jacobians) const override

virtual Eigen::VectorXd	evaluateResiduals (const std::vector< Eigen::Map< const Eigen::VectorXd > > &_st_vector) const =0
	Returns the residual evaluated in the states provided. More...

virtual void	evaluateJacobians (const std::vector< Eigen::Map< const Eigen::VectorXd > > &_st_vector, std::vector< Eigen::Map< Eigen::MatrixRowXd > > &jacobians, const std::vector< bool > &_compute_jacobian) const =0
	Returns the normalized jacobians evaluated in the states. More...

virtual void	evaluateJacobians (const std::vector< Eigen::Map< const Eigen::VectorXd > > &_st_vector, std::vector< Eigen::MatrixXd > &jacobians, const std::vector< bool > &_compute_jacobian) const =0

virtual void	evaluateUnweightedJacobians (std::vector< Eigen::MatrixXd > &jacobians) const =0
	Returns the jacobians (without measurement noise normalization) evaluated in the current state blocks values. More...

Public Member Functions inherited from wolf::FactorBase
	FactorBase (const std::string &_type, const FactorTopology &_top, const JacobianMethod &_jac_method, const Eigen::VectorXd &_measurement, const Eigen::MatrixXd &_measurement_sqrt_information_upper, const NodeStateBlocksPtrList &_node_state_block_list, const ProcessorBasePtr &_processor_ptr, const std::vector< StateBlockPtr > &_state_ptrs, bool _apply_loss_function, FactorStatus _status=FAC_ACTIVE)
	constructor from measurement and noise square root information upper matrix (cholesky) More...

	~FactorBase () override=default

virtual void	remove (bool viral_remove_parent_without_children=false)

bool	hasChildren () const override

unsigned int	id () const

virtual FactorTopology	getTopology () const final
	get the main topological characteristic More...

virtual void	getTopology (FactorTopology _top) final
	set the main topological characteristic More...

std::string	getTopologyString () const
	get the main topological characteristic as string More...

virtual bool	evaluate (double const const _parameters, double _residuals, double *_jacobians) const =0
	Evaluate the factor given the input parameters and returning the residuals and jacobians. More...

virtual bool	evaluate (const std::vector< const double * > &_states_ptr, Eigen::VectorXd &_residual, std::vector< Eigen::MatrixXd > &_jacobians) const =0

virtual Eigen::VectorXd	residual () const
	Evaluate the factor residual with the current state block values. More...

virtual double	cost () const
	Evaluate the cost of the factor with the current state block values. More...

virtual JacobianMethod	getJacobianMethod () const final
	Returns the jacobians computation method. More...

virtual std::vector< StateBlockConstPtr >	getStateBlockPtrVector () const
	Returns a vector of pointers to the states in which this factor depends. More...

virtual std::vector< StateBlockPtr >	getStateBlockPtrVector ()

virtual std::vector< unsigned int >	getStateSizes () const
	Returns a vector of the states sizes. More...

virtual const Eigen::VectorXd &	getMeasurement () const
	Returns a reference to the measurement. More...

virtual const Eigen::MatrixXd &	getMeasurementSquareRootInformationUpper () const
	Returns a reference to the measurement square root information. More...

FeatureBaseConstPtr	getFeature () const
	Returns a pointer to the feature constrained from. More...

FeatureBasePtr	getFeature ()

CaptureBaseConstPtr	getCapture () const
	Returns a pointer to its capture. More...

CaptureBasePtr	getCapture ()

FrameBaseConstPtr	getFrame () const
	Returns a pointer to its frame. More...

FrameBasePtr	getFrame ()

ProcessorBaseConstPtr	getProcessor () const
	getProcessor More...

ProcessorBasePtr	getProcessor ()

virtual unsigned int	getSize () const =0
	Returns the factor residual size. More...

FactorStatus	getStatus () const
	Gets the status. More...

void	setStatus (FactorStatus _status)
	Sets the status. More...

bool	getApplyLossFunction () const
	Gets the apply loss function flag. More...

NodeStateBlocksWPtrList	getNodesFactored ()

FrameBaseWPtrList	getFramesFactored ()

CaptureBaseWPtrList	getCapturesFactored ()

LandmarkBaseWPtrList	getLandmarksFactored ()

SensorBaseWPtrList	getSensorsFactored ()

NodeStateBlocksConstWPtrList	getNodesFactored () const

FrameBaseConstWPtrList	getFramesFactored () const

CaptureBaseConstWPtrList	getCapturesFactored () const

LandmarkBaseConstWPtrList	getLandmarksFactored () const

SensorBaseConstWPtrList	getSensorsFactored () const

bool	hasNode (const NodeStateBlocksConstPtr &_nsb) const

bool	hasFrame (const FrameBaseConstPtr &_frm) const

bool	hasCapture (const CaptureBaseConstPtr &_cap) const

bool	hasLandmark (const LandmarkBaseConstPtr &_lmk) const

bool	hasSensor (const SensorBaseConstPtr &_sen) const

void	link (FeatureBasePtr ftr)

virtual void	printHeader (int _depth, bool _factored_by, bool _metric, bool _state_blocks, std::ostream &_stream, std::string _tabs="") const

void	print (int _depth, bool _factored_by, bool _metric, bool _state_blocks, std::ostream &_stream=std::cout, std::string _tabs="") const

virtual CheckLog	localCheck (bool _verbose, std::ostream &_stream, std::string _tabs="") const

bool	check (CheckLog &_log, bool _verbose, std::ostream &_stream, std::string _tabs="") const

Public Member Functions inherited from wolf::NodeBase
	NodeBase (const std::string &_category, const std::string &_type="Undefined", const std::string &_name="")

virtual	~NodeBase ()=default

unsigned int	nodeId () const

std::string	getCategory () const

std::string	getType () const

std::string	getName () const

bool	isRemoving () const

virtual bool	hasChildren () const =0

void	setType (const std::string &_type)

void	setName (const std::string &_name)

ProblemConstPtr	getProblem () const

ProblemPtr	getProblem ()

Additional Inherited Members
Static Public Member Functions inherited from wolf::FactorBase
template<typename classType , typename... T>
static std::shared_ptr< classType >	emplace (FeatureBasePtr _oth_ptr, T &&... all)

Protected Member Functions inherited from wolf::FactorBase
void	setProblem (ProblemPtr) override final

virtual void	setProblem (ProblemPtr _prob_ptr)

Protected Attributes inherited from wolf::FactorBase
unsigned int	factor_id_

FactorTopology	topology_
	topology of factor More...

JacobianMethod	jacobian_method_
	jacobian method More...

FactorStatus	status_
	status of factor More...

bool	apply_loss_function_
	flag for applying loss function to this factor More...

Eigen::VectorXd	measurement_
	the measurement vector More...

Eigen::MatrixXd	measurement_sqrt_information_upper_
	the squared root information matrix More...

std::vector< unsigned int >	state_block_sizes_
	vector containing sizes of the state blocks involved More...

std::vector< StateBlockPtr >	state_block_vector_
	vector containing the state blocks involved More...

Protected Attributes inherited from wolf::NodeBase
ProblemWPtr	problem_ptr_

unsigned int	node_id_
	Node id. It is unique over the whole Wolf Tree. More...

std::string	node_category_
	Text label identifying the category of node ("SENSOR", "FEATURE", etc) More...

std::string	node_type_
	Text label identifying the type or subcategory of node ("Pin Hole", "Point 2d", etc) More...

std::string	node_name_

bool	is_removing_
	A flag for safely removing nodes from the Wolf tree. See remove(). More...

Constructor & Destructor Documentation

◆ FactorAnalytic()

wolf::FactorAnalytic::FactorAnalytic	(	const std::string &	_type,
		const FactorTopology &	_top,
		const Eigen::VectorXd &	_measurement,
		const Eigen::MatrixXd &	_measurement_sqrt_information_upper,
		const NodeStateBlocksPtrList &	_node_state_block_list,
		const ProcessorBasePtr &	_processor_ptr,
		const std::vector< StateBlockPtr > &	_state_ptrs,
		bool	_apply_loss_function,
		FactorStatus	_status
	)

◆ ~FactorAnalytic()

wolf::FactorAnalytic::~FactorAnalytic ( )

overridedefault

Member Function Documentation

◆ evaluate() [1/2]

bool wolf::FactorAnalytic::evaluate	(	const std::vector< const double * > &	_states_ptr,
		Eigen::VectorXd &	_residual,
		std::vector< Eigen::MatrixXd > &	_jacobians
	)		const

overridevirtual

Returns the residual vector and a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr

Implements wolf::FactorBase.

◆ evaluate() [2/2]

bool wolf::FactorAnalytic::evaluate	(	double const const	parameters,
		double *	residuals,
		double **	jacobians
	)		const

overridevirtual

Evaluate the factor given the input parameters and returning the residuals and jacobians.

Implements wolf::FactorBase.

◆ evaluateJacobians() [1/2]

virtual void wolf::FactorAnalytic::evaluateJacobians	(	const std::vector< Eigen::Map< const Eigen::VectorXd > > &	_st_vector,
		std::vector< Eigen::Map< Eigen::MatrixRowXd > > &	jacobians,
		const std::vector< bool > &	_compute_jacobian
	)		const

pure virtual

Returns the normalized jacobians evaluated in the states.

Returns the normalized (by the measurement noise sqrt information) jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXd. IMPORTANT: only fill the jacobians of the state blocks specified in _compute_jacobian.

Parameters

_st_vector	is a vector containing the mapped eigen vectors of all state blocks involved in the factor
jacobians	is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
_compute_jacobian	is a vector that specifies whether the ith jacobian sould be computed or not

Implemented in wolf::FactorBlockAbsolute, wolf::FactorBlockDifference, wolf::FactorRelativePose2d, and wolf::FactorRelativePosition2d.

◆ evaluateJacobians() [2/2]

virtual void wolf::FactorAnalytic::evaluateJacobians	(	const std::vector< Eigen::Map< const Eigen::VectorXd > > &	_st_vector,
		std::vector< Eigen::MatrixXd > &	jacobians,
		const std::vector< bool > &	_compute_jacobian
	)		const

pure virtual

Implemented in wolf::FactorBlockAbsolute, wolf::FactorBlockDifference, wolf::FactorRelativePose2d, and wolf::FactorRelativePosition2d.

◆ evaluateResiduals()

virtual Eigen::VectorXd wolf::FactorAnalytic::evaluateResiduals ( const std::vector< Eigen::Map< const Eigen::VectorXd > > & _st_vector ) const

pure virtual

Returns the residual evaluated in the states provided.

Returns the residual evaluated in the states provided in std::vector of mapped Eigen::VectorXd

Implemented in wolf::FactorBlockAbsolute, wolf::FactorBlockDifference, wolf::FactorRelativePose2d, and wolf::FactorRelativePosition2d.

◆ evaluateUnweightedJacobians()

virtual void wolf::FactorAnalytic::evaluateUnweightedJacobians ( std::vector< Eigen::MatrixXd > & jacobians ) const

pure virtual

Returns the jacobians (without measurement noise normalization) evaluated in the current state blocks values.

Returns the jacobians (without measurement noise normalization) evaluated in the current state blocks values

Parameters

jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block

Implemented in wolf::FactorBlockAbsolute, wolf::FactorBlockDifference, wolf::FactorRelativePose2d, and wolf::FactorRelativePosition2d.

The documentation for this class was generated from the following files:

include/core/factor/factor_analytic.h
src/factor/factor_analytic.cpp

Public Member Functions

Additional Inherited Members

Constructor & Destructor Documentation

◆ FactorAnalytic()

◆ ~FactorAnalytic()

Member Function Documentation

◆ evaluate() [1/2]

◆ evaluate() [2/2]

◆ evaluateJacobians() [1/2]

◆ evaluateJacobians() [2/2]

◆ evaluateResiduals()

◆ evaluateUnweightedJacobians()